﻿Imports Model
Imports Model.Geometry
Imports Model.Midline
Imports Helper
Imports Model.Section

Public Class Section

    ''' <summary>
    ''' 点到平面的距离
    ''' </summary>
    ''' <param name="pl">平面</param>
    ''' <param name="pt">点集合</param>
    ''' <returns>返回点到平面的距离</returns>
    ''' <remarks></remarks>
    Public Shared Function PointToPlane(ByVal pl As Plane, ByVal pt As Point3d) As Double

        Dim result As Double
        Dim temp1 = Math.Abs(pl.praA * pt.p_x + pl.praB * pt.p_y + pl.praC * pt.p_z + pl.praD)
        Dim temp2 = Math.Sqrt(pl.praA * pl.praA + pl.praB * pl.praB + pl.praC * pl.praC)
        result = temp1 / temp2
        Return result

    End Function

    ''' <summary>
    ''' 点投影到平面,已完成,经过测试
    ''' </summary>
    ''' <param name="pl">垂面</param>
    ''' <param name="pt">获取的点集合</param>
    ''' <returns>投影后的点集合</returns>
    ''' <remarks></remarks>
    Public Shared Function ProjectionToPlane(ByVal pl As Plane, ByVal pt As Point3d) As Point3d

        Dim temp_1 = (pl.praA * pt.p_x + pl.praB * pt.p_y + pl.praC * pt.p_z + pl.praD)
        Dim temp_2 = (pl.praA * pl.praA + pl.praB * pl.praB + pl.praC * pl.praC)
        Dim temp_3 = -1 * temp_1 / temp_2
        Dim result As Point3d = New Point3d()

        result.p_x = pt.p_x + pl.praA * temp_3
        result.p_y = pt.p_y + pl.praB * temp_3
        result.p_z = pt.p_z + pl.praC * temp_3

        Return result

    End Function

    ''' <summary>
    ''' 圆形拟合_最小二乘法,已完成,经过测试
    ''' </summary>
    ''' <param name="ptlist">投影并简化的点集合</param>
    ''' <param name="cir">拟合圆形结果</param>
    ''' <returns>真假，无实意</returns>
    ''' <remarks></remarks>
    Public Shared Function LS_CircleFitting(ByVal ptlist As List(Of Point3d), ByRef cir As Circle) As Boolean

        Dim ResultCircle As New Circle()
        ResultCircle.radius = 0.0F
        ResultCircle.center.p_x = 0.0F
        ResultCircle.center.p_y = 0.0F
        ResultCircle.center.p_z = 0.0F

        If ptlist.Count < 3 Then
            MsgBox("拟合数据点数不足，无法完成拟合")
            Return Nothing
        End If

        Dim sum_x = 0.0F, sum_y = 0.0F
        Dim sum_x2 = 0.0F, sum_y2 = 0.0F
        Dim sum_x3 = 0.0F, sum_y3 = 0.0F
        Dim sum_xy = 0.0F, sum_x1y2 = 0.0F, sum_x2y1 = 0.0F

        Dim ptNum As Integer = ptlist.Count
        Dim i As Integer
        For i = 0 To ptlist.Count - 1 Step 1
            Dim x As Double = ptlist(i).p_x
            Dim y As Double = ptlist(i).p_y
            Dim x2 As Double = x * x
            Dim y2 As Double = y * y
            sum_x += x
            sum_y += y
            sum_x2 += x2
            sum_y2 += y2
            sum_x3 += x2 * x
            sum_y3 += y2 * y
            sum_xy += x * y
            sum_x1y2 += x * y2
            sum_x2y1 += x2 * y
        Next

        Dim C As Double, D As Double, E As Double, G As Double, H As Double

        Dim a As Double, b As Double, cc As Double

        C = ptNum * sum_x2 - sum_x * sum_x
        D = ptNum * sum_xy - sum_x * sum_y
        E = ptNum * sum_x3 + ptNum * sum_x1y2 - (sum_x2 + sum_y2) * sum_x
        G = ptNum * sum_y2 - sum_y * sum_y
        H = ptNum * sum_x2y1 + ptNum * sum_y3 - (sum_x2 + sum_y2) * sum_y
        a = (H * D - E * G) / (C * G - D * D)
        b = (H * C - E * D) / (D * D - G * C)
        cc = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / ptNum

        cir.center.p_x = a / (-2)
        cir.center.p_y = b / (-2)
        'Dim r = a * a + b * b - 4 * C
        cir.radius = Math.Sqrt(Math.Abs(a * a + b * b - 4 * cc)) / 2

        Return Nothing

    End Function

    ''' <summary>
    ''' 将点坐标转化为以中线点为原点的坐标值,垂直法线的水平线为X轴,高程为Y轴,已完成,经过测试
    ''' 适用于Z轴没有倾斜的情况
    ''' </summary>
    ''' <param name="pt">当前里程点</param>
    ''' <param name="frompt">获取的投影点云</param>
    ''' <param name="topt" >坐标转换后的点云</param>
    ''' <returns>真假,无实意</returns>
    ''' <remarks></remarks>
    Public Shared Function SimpleCoord(ByRef Midline As Mid_Line, ByRef pt As Point3d, ByRef frompt As List(Of Point3d), ByRef topt As List(Of Point3d)) As Boolean

        Dim Num = frompt.Count
        Dim temppt As Point3d
        topt.Clear()
        For i = 0 To Num - 1
            temppt = New Point3d()
            If isOnRightofLine(Midline, frompt(i)) Then
                temppt.p_x = pt.GetDis2D(frompt(i))
            Else
                temppt.p_x = -1 * pt.GetDis2D(frompt(i))
            End If
            temppt.p_y = frompt(i).p_z
            temppt.p_z = 0
            temppt.p_flag = 0
            topt.Add(temppt)
        Next
        Return True
    End Function

    ''' <summary>
    ''' 将点坐标转化为以中线点为原点的坐标值,垂直法线的水平线为X轴,高程为Y轴,已完成,经过测试
    ''' 适用于Z轴没有倾斜的情况
    ''' </summary>
    ''' <param name="pt">当前里程点</param>
    ''' <param name="frompt">获取的投影点云</param>
    ''' <param name="topt" >坐标转换后的点云</param>
    ''' <returns>真假,无实意</returns>
    ''' <remarks></remarks>
    Public Shared Function SimpleCoord(ByRef vec As SingleVector, ByRef pt As Point3d, ByRef frompt As List(Of Point3d), ByRef topt As List(Of Point3d)) As Boolean

        Dim Num = frompt.Count
        Dim temppt As Point3d
        topt.Clear()
        For i = 0 To Num - 1
            temppt = New Point3d()
            If isOnRightofLine2(vec, pt, frompt(i)) Then
                temppt.p_x = pt.GetDis2D(frompt(i))
            Else
                temppt.p_x = -1 * pt.GetDis2D(frompt(i))
            End If
            temppt.p_y = frompt(i).p_z
            temppt.p_z = 0
            temppt.p_flag = 0
            topt.Add(temppt)
        Next
        Return True

    End Function

    ''' <summary>
    ''' 获得真实坐标
    ''' </summary>
    ''' <param name="vec"></param>
    ''' <param name="falsept"></param>
    ''' <param name="oript" ></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Public Shared Function GetTrueCoord(ByVal vec As SingleVector, ByRef falsept As Point3d, ByVal oript As Point3d) As Point3d
        ''获取某点的真实坐标
        Dim thea = Math.Atan2(vec.V_y, vec.V_x)

        Dim temppt As New Point3d()
        temppt.p_x = falsept.p_x
        temppt.p_y = falsept.p_z
        temppt.p_z = falsept.p_y

        Dim ptmat(2, 0) As Double
        ptmat(0, 0) = temppt.p_x
        ptmat(1, 0) = temppt.p_y
        ptmat(2, 0) = temppt.p_z
        Dim translate(2, 0) As Double
        translate(0, 0) = oript.p_x
        translate(1, 0) = oript.p_y
        translate(2, 0) = 0

        Dim mat(2, 2) As Double
        mat(0, 0) = Math.Cos(thea - Math.PI / 2)
        mat(0, 1) = -Math.Sin(thea - Math.PI / 2)
        mat(0, 2) = 0
        mat(1, 0) = Math.Sin(thea - Math.PI / 2)
        mat(1, 1) = Math.Cos(thea - Math.PI / 2)
        mat(0, 1) = 0
        mat(2, 0) = 0
        mat(2, 1) = 0
        mat(2, 2) = 1

        Dim result(,) = MatLib.Add(translate, MatLib.Multiply(mat, ptmat))
        Dim resultpt As New Point3d()
        resultpt.p_x = result(0, 0)
        resultpt.p_y = result(1, 0)
        resultpt.p_z = result(2, 0)

        Return resultpt
    End Function

    ''' <summary>
    ''' 给定点和向量，求平面
    ''' </summary>
    ''' <param name="pt"></param>
    ''' <param name="vec"></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Public Shared Function GetPlane(ByVal pt As Point3d, ByVal vec As SingleVector) As Plane
        Dim currplane As Plane = New Plane()
        currplane.praA = vec.V_x
        currplane.praB = vec.V_y
        currplane.praC = vec.V_z
        currplane.praD = -1 * (currplane.praA * pt.p_x + currplane.praB * pt.p_y + currplane.praC * pt.p_z)
        currplane.normal_vector.V_x = vec.V_x
        currplane.normal_vector.V_y = vec.V_y
        currplane.normal_vector.V_z = vec.V_z
        Return currplane
    End Function

    ''' <summary>
    ''' 简单去除隧道内外壁的噪声点,待完成
    ''' </summary>
    ''' <param name="ptlist">投影之后的点集合</param>
    ''' <returns>真假，无实意</returns>
    ''' <remarks></remarks>
    Public Shared Function FilterPoint(ByRef ptlist As List(Of Point3d)) As Boolean

        'Dim avg_x As Double = 0
        'Dim avg_y As Double = 0
        'Dim avg_z As Double = 0

        'Dim i = 0
        'Dim n = ptlist.Count
        'For i = 0 To n - 1
        '    avg_x += ptlist(i).p_x
        '    avg_y += ptlist(i).p_y
        '    avg_z += ptlist(i).p_z
        'Next

        'avg_x = avg_x / n
        'avg_y = avg_y / n
        'avg_z = avg_z / n

        'For 
        Return True

    End Function

    Public Shared Function HorseFitting(ByVal ptlist_up As List(Of Point3d), ByVal ptlist_down As List(Of Point3d), ByRef cir_up As Circle, ByRef cir_down As Circle)
        LS_CircleFitting(ptlist_up, cir_up)
        LS_CircleFitting(ptlist_down, cir_down)
        Return Nothing
    End Function

    Public Shared Function RectangleProcess(ByRef rr As RectangleResult, ByRef prt As ParaRectangleTunnel)
        Dim ptleft As List(Of Point3d)
        Dim ptup As List(Of Point3d)
        Dim ptdown As List(Of Point3d)
        Dim ptzt As List(Of Point3d)

        Dim rd = New Random()

        ptleft = New List(Of Point3d)()
        ptup = New List(Of Point3d)()
        ptdown = New List(Of Point3d)()
        ptzt = New List(Of Point3d)()

        Dim designLeft, designUp, designDown As Double
        Dim designZTH, designZTV As Double
        designLeft = prt.CeGG
        designUp = prt.DingGG + rr.stand_ele
        designDown = rr.stand_ele - prt.DiGG

        designZTH = rr.design_ZTH
        designZTV = rr.design_ZTV + rr.stand_ele

        ptleft = Getleftpt(rr.false_ptlist, designLeft)
        ptup = Getuppt(rr.false_ptlist, designUp)
        ptdown = Getdownpt(rr.false_ptlist, designDown)
        ptzt = Getztpt(rr.false_ptlist, designZTH, designZTV)

        Dim upvalue, downvalue As Double
        Dim leftvalue(3) As Double
        Dim zthvalue, ztvvalue As Double

        leftvalue = FitVerticalLine(ptleft, rr.stand_ele)
        upvalue = FitHorizontalLine(ptup)
        downvalue = FitHorizontalLine(ptdown)
        zthvalue = FitVerticalLine(ptzt)
        ztvvalue = Findmax_z(ptzt) - rr.stand_ele

        rr.difCeGG = Math.Abs(leftvalue(0))
        rr.difCeGG_21 = Math.Abs(leftvalue(1))
        rr.difCeGG_42 = Math.Abs(leftvalue(2))

        rr.max_ele = upvalue
        rr.min_ele = downvalue
        rr.difmax_ele = rr.max_ele - rr.stand_ele
        rr.difmin_ele = rr.stand_ele - rr.min_ele
        rr.dif_zth = zthvalue
        rr.dif_ztv = ztvvalue + 0.03


        Return Nothing
    End Function

    Private Shared Function Getleftpt(false_ptlist As List(Of Point3d), designLeft As Double) As List(Of Point3d)
        Dim xmin As Double = -designLeft - 0.1
        Dim xmax As Double = -designLeft + 0.1
        Dim leftlist = New List(Of Point3d)()
        Dim pt As Point3d

        leftlist.Clear()
        For i = 0 To false_ptlist.Count - 1
            pt = New Point3d()
            If false_ptlist(i).p_x > xmin And false_ptlist(i).p_x < xmax Then
                pt = false_ptlist(i)
                leftlist.Add(pt)
            End If

        Next

        Return leftlist

    End Function

    Private Shared Function Getuppt(false_ptlist As List(Of Point3d), designUp As Double) As List(Of Point3d)
        Dim ymin As Double = designUp - 0.1
        Dim ymax As Double = designUp + 0.1
        Dim uplist = New List(Of Point3d)()
        Dim pt As Point3d

        uplist.Clear()
        For i = 0 To false_ptlist.Count - 1
            pt = New Point3d()
            If false_ptlist(i).p_y > ymin And false_ptlist(i).p_y < ymax Then
                pt = false_ptlist(i)
                uplist.Add(pt)
            End If

        Next

        Return uplist
    End Function

    Private Shared Function Getdownpt(false_ptlist As List(Of Point3d), designDown As Double) As List(Of Point3d)

        Dim ymin As Double = designDown - 0.1
        Dim ymax As Double = designDown + 0.1
        Dim downlist = New List(Of Point3d)()
        Dim pt As Point3d

        downlist.Clear()
        For i = 0 To false_ptlist.Count - 1
            pt = New Point3d()
            If false_ptlist(i).p_y > ymin And false_ptlist(i).p_y < ymax Then
                pt = false_ptlist(i)
                downlist.Add(pt)
            End If
        Next
        Return downlist

    End Function

    Private Shared Function FitVerticalLine(ptleft As List(Of Point3d), ele As Double) As Double()
        Dim tempx() As Double = {0, 0, 0}
        Dim tempnum() As Integer = {0, 0, 0}
        For index = 0 To ptleft.Count - 1

            Select Case (ptleft(index).p_y - ele)
                Case Is > 4.2
                    tempx(2) += ptleft(index).p_x
                    tempnum(2) += 1
                Case Is < 0
                    tempx(0) += ptleft(index).p_x
                    tempnum(0) += 1
                Case 2.1 To 4.2
                    tempx(2) += ptleft(index).p_x
                    tempnum(2) += 1
                    tempx(1) += ptleft(index).p_x
                    tempnum(1) += 1
                Case 0 To 2.1
                    tempx(0) += ptleft(index).p_x
                    tempnum(0) += 1
                    tempx(1) += ptleft(index).p_x
                    tempnum(1) += 1
            End Select

        Next
        tempx(0) = tempx(0) / tempnum(0)
        tempx(1) = tempx(1) / tempnum(1)
        tempx(2) = tempx(2) / tempnum(2)
        Return tempx
    End Function

    Private Shared Function FitVerticalLine(ptleft As List(Of Point3d)) As Double
        Dim tempx As Double = 0.0
        For index = 0 To ptleft.Count - 1
            tempx += ptleft(index).p_x
        Next
        tempx = tempx / ptleft.Count
        Return tempx
    End Function

    Private Shared Function FitHorizontalLine(ptup As List(Of Point3d)) As Double
        Dim tempy As Double = 0.0
        For index = 0 To ptup.Count - 1
            tempy += ptup(index).p_y
        Next
        tempy = tempy / ptup.Count
        Return tempy
    End Function

    Private Shared Function Getztpt(false_ptlist As List(Of Point3d), designZTH As Double, designZTV As Double) As List(Of Point3d)
        Dim ymin As Double = designZTV - 0.1 - 0.2
        Dim ymax As Double = designZTV + 0.1
        Dim xmin As Double = designZTH - 0.1
        Dim xmax As Double = designZTH + 0.1
        Dim ztlist = New List(Of Point3d)()
        Dim pt As Point3d

        ztlist.Clear()
        For i = 0 To false_ptlist.Count - 1
            pt = New Point3d()
            If false_ptlist(i).p_y > ymin And false_ptlist(i).p_y < ymax And false_ptlist(i).p_x > xmin And false_ptlist(i).p_x < xmax Then
                pt = false_ptlist(i)
                ztlist.Add(pt)
            End If
        Next
        Return ztlist

    End Function

    Private Shared Function Findmax_z(ptzt As List(Of Point3d)) As Double
        Dim temp As Double = 0.0
        For index = 0 To ptzt.Count - 1
            If temp < ptzt(index).p_y Then
                temp = ptzt(index).p_y
            End If
        Next
        Return temp
    End Function


End Class